#include <iostream>
#include <string>
#include <cmath>
#include <opencv2/opencv.hpp>
#include <stdlib.h>
#include <stdio.h>
using namespace std;
using namespace cv;

void find_corners(vector<Point2f> &, vector<Point2f> &, Rect &, RotatedRect &, Mat &, vector<vector<Point>> &, int); //找出角点//
int max_y(vector<Point2f> &);                                                                                        //找出每个灯条最上方的角点//
int min_y(vector<Point2f> &);
int h; //找出每个灯条最下方的角点//

int main(int argc, char *argv[])
{
    Mat src_img;
    Mat hsv_img;
    Mat output_img;
    Mat inRange_img;
    Mat canny_img;
    Mat erode_img;
    Mat dilate_img;
    Mat contours_img;
    Mat struct_element1 = getStructuringElement(MORPH_RECT, Size(3, 3), Point(-1, -1));
    Mat struct_element2 = getStructuringElement(MORPH_RECT, Size(13, 13), Point(-1, -1));
    RNG rng(12345); //随机产生颜色//
    vector<vector<Point>> contours;
    vector<Point2f> corners1; //储存每个灯条所有角点//
    vector<Point2f> corners2;
    vector<Point2f> light_bar1(2); //储存每个灯条上下两个角点//
    vector<Point2f> light_bar2(2);
    vector<Vec4i> hierarchy;
    RotatedRect ellipse0;
    RotatedRect ellipse1;
    int hmax = 130, hmin = 80;
    int vmax = 255, vmin = 50;
    int smax = 255, smin = 42;
    int x;
    int y;
    int macth_num;
    int min_num = -1;
    int width; //装甲片的长和宽//
    int height;
    int x_diff; //两个轮廓的水平距离//
    double scale3;
    float current_scale;
    float min_scale = 1.2;
    float rect_scale0;
    float rect_scale1;
    float rect_scale3;

    VideoCapture cap("/home/joker/Desktop/armor.mp4");

    while (1)
    {
        if (!cap.isOpened())
        {
            cout << "No video is opened!";
        }

        cap >> src_img;
        cap >> output_img;
        Mat back_ground = Mat::zeros(src_img.size(), CV_8UC3);

        if (src_img.empty() || output_img.empty())
        {
            return 0;
        }
        cvtColor(output_img, hsv_img, COLOR_BGR2HSV);
        inRange(hsv_img, Scalar(hmin, smin, vmin), Scalar(hmax, smax, vmax), inRange_img);
        Canny(inRange_img, canny_img, 222, 400);
        erode(canny_img, canny_img, struct_element1);
        findContours(inRange_img, contours, hierarchy, RETR_EXTERNAL, CHAIN_APPROX_NONE);

        vector<Rect> boundRect(contours.size()); //矩形逼近每个轮廓//
        for (int i = 0; i < contours.size(); i++)
        {
            boundRect[i] = boundingRect(Mat(contours[i]));
        }

        for (int i = 0; i < contours.size(); i++) //遍历每个轮廓
        {
            Scalar color = Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)); //每个装甲片用不同颜色标记//
            min_scale = 1.2;
            min_num = -1;
            for (int j = 0; j < contours.size(); j++) //通过筛选挑出符合要求的轮廓//
            {
                x_diff = fabs(boundRect[i].x - boundRect[j].x);
                rect_scale0 = (float)boundRect[i].height / boundRect[i].width; //每个轮廓的长宽比//
                rect_scale1 = (float)boundRect[j].height / boundRect[j].width;
                rect_scale3 = (float)boundRect[i].height / boundRect[j].height; //两个轮廓之间的长度比//

                if (rect_scale3 <= 1)
                {
                    rect_scale3 = 1.0 / rect_scale3;
                }
                if (i != j && x_diff >= 30 && rect_scale0 >= 1.0 && rect_scale1 >= 1.0 && contours[i].size() > 20 && contours[j].size() > 20 && rect_scale3 <= 1.2)
                {
                    float area_scale = contourArea(contours[i]) / contourArea(contours[j]);
                    if (area_scale <= 1)
                    {
                        area_scale = 1.0 / area_scale;
                    }
                    float distance_y = fabs(boundRect[i].y + boundRect[i].height - boundRect[j].y - boundRect[j].height); //两个轮廓之间的高度差//
                    if (distance_y <= 20 && area_scale <= 2)
                    {
                        ellipse0 = fitEllipse(contours[i]); //椭圆逼近获取每个轮廓中心和倾斜度//
                        ellipse1 = fitEllipse(contours[j]);
                        current_scale = ((ellipse0.angle >= 170 ? 180 - ellipse0.angle : ellipse0.angle) + 50) /
                                        ((ellipse1.angle >= 170 ? 180 - ellipse1.angle : ellipse1.angle) + 50); //两轮廓的倾斜度之比，加50来防止比例过大//
                        if (current_scale < 1)                                                                  //比例小于1的话取倒数，方便设置阈值//
                        {
                            current_scale = 1.0 / current_scale;
                        }
                        if (current_scale <= min_scale) //循遍j个轮廓，找出倾斜度之比最小的//
                        {
                            min_scale = current_scale;
                            min_num = j; //记录下倾斜度比最小的轮廓的下角标//
                        }
                    }
                }
            }
            if (min_num != -1) //min_num处值为-1，直到找到一个倾斜度比例小于阈值的轮廓才发生改变//
            {
                if (boundRect[i].x > boundRect[min_num].x) //挑选出两个轮廓最靠左上角的点//
                {
                    x = boundRect[min_num].x;
                    y = boundRect[min_num].y;
                }
                else
                {
                    x = boundRect[i].x;
                    y = boundRect[i].y;
                }
                width = abs(boundRect[i].x - boundRect[min_num].x) + boundRect[i].width; //计算装甲片的长和宽//
                height = boundRect[i].height;
                Rect bind_Rect(x, y, width, height);
                scale3 = (double)(width) / height;
                if (scale3 >= 2 && scale3 <= 5)
                {
                    Point2i center(bind_Rect.x + 0.5 * bind_Rect.width, bind_Rect.y + 0.5 * bind_Rect.height);
                    circle(src_img, Point2f(bind_Rect.x, bind_Rect.y), 1, Scalar(255, 255, 255), 1);//标出角点//
                    circle(src_img, Point2f(bind_Rect.x + bind_Rect.width, bind_Rect.y + bind_Rect.height), 1, Scalar(255, 255, 255), 1);
                    circle(src_img, Point2f(bind_Rect.x, bind_Rect.y + bind_Rect.height), 1, Scalar(255, 255, 255), 1);
                    circle(src_img, Point2f(bind_Rect.x + bind_Rect.width, bind_Rect.y), 1, Scalar(255, 255, 255), 1);
                    circle(src_img, center, 1, Scalar(255, 255, 255), 1); //标出装甲片中心//
                    contours[min_num] = {{(5, 5)}};                       //把已经匹配的轮廓改变，以防重复匹配//
                    line(src_img, Point2f(bind_Rect.x, bind_Rect.y), Point2f(bind_Rect.x + bind_Rect.width, bind_Rect.y + bind_Rect.height), Scalar(255, 255, 255));
                    line(src_img, Point2f(bind_Rect.x, bind_Rect.y + bind_Rect.height), Point2f(bind_Rect.x + bind_Rect.width, bind_Rect.y), Scalar(255, 255, 255));
                    // find_corners(corners1, light_bar1, boundRect[min_num], ellipse1, src_img, contours, min_num);
                    // find_corners(corners2, light_bar2, boundRect[i], ellipse0, src_img, contours, i);
                    // rectangle(src_img, bind_Rect, Scalar(255, 255, 255), 1); //用矩形标记装甲片
                    // rectangle(back_ground, bind_Rect, Scalar(255, 255, 255), 1);
                    // if (h)
                    // {
                    //     line(src_img, light_bar1[0], light_bar2[1], Scalar(255, 255, 255));
                    //     line(src_img, light_bar1[1], light_bar2[0], Scalar(255, 255, 255));
                    // }
                }
            }
        }

        imshow("src_video", src_img);
        waitKey(30);
    }
    return 0;
}

void find_corners(vector<Point2f> &corner, vector<Point2f> &light_bar, Rect &boundRect, RotatedRect &ellipse0,
                  Mat &src_img, vector<vector<Point>> &contours, int con_num)
{
    int max, min;
    Rect rect_0, rect;
    Mat back_ground = Mat::zeros(src_img.size(), CV_8UC3); //在back_ground显示椭圆逼近的结果，使角点更清晰//
    Mat mask = Mat::zeros(back_ground.size(), CV_8UC1);    //用掩膜限制范围，减少计算量//

    rect_0.x = boundRect.x; //将矩形掩膜适当放大，过拟合可能没有包括边缘的角点//
    rect_0.y = boundRect.y;
    rect_0.width = boundRect.width;
    rect_0.height = boundRect.height;

    mask(rect_0).setTo(255);
    ellipse(back_ground, ellipse0, Scalar(255, 255, 255));
    cvtColor(back_ground, back_ground, COLOR_BGR2GRAY);
    goodFeaturesToTrack(back_ground, corner, 100, 0.01, 10, mask, 3, false, 0.04); //角点检测//
    if (corner.size() == 0)
    {
        h = 0;
        return;
    }
    max = max_y(corner);
    min = min_y(corner);

    light_bar[0].x = corner[max].x;
    light_bar[0].y = corner[max].y;
    light_bar[1].x = corner[min].x;
    light_bar[1].y = corner[min].y;
    h = 1;
}

int min_y(vector<Point2f> &corner)
{
    int min = 0;
    int min_0 = corner[0].y;
    for (size_t i = 0; i < corner.size(); i++)
    {
        if (corner[i].y < min_0)
        {
            min = i;
            min_0 = corner[i].y;
        }
    }
    return min;
}

int max_y(vector<Point2f> &corner)
{
    int max = 0;
    int max_0 = corner[0].y;
    for (size_t i = 0; i < corner.size(); i++)
    {
        if (corner[i].y > max_0)
        {
            max = i;
            max_0 = corner[i].y;
        }
    }
    return max;
}